#include <Wire.h>
#include <Servo.h>
#include <MiniBot.h>
int dfultright;

Servo rightmotor;
Servo leftmotor;
int oldfront = 0;

MiniBot bot;          // declare minibot object "bot"

//
//declare all target validation functions
//
bool Checklight();      //Checks data from the light sensors
void targetfound();     //Stops the minibot and flashes LED, once target is found

//
//declare all motor functions
//
void moveforward(int leftvalue, int rightvalue);     //Moves minibot forward
void adjustright();     //Adjusts minibot to the right
void adjustleft();      //Adjusts minibot to the left
void turnright();       //Turns the minibot to the right and moves forward one unit
void turnleft();        //Turns the minibot to the left and moves forward one unit
void turnaround();      //Turns the minibot around
void oops_goright();
void oops_goleft();



void setup()
{
    bot.Setup();        // Initializes all sensors
    rightmotor.attach(11);
    leftmotor.attach(10);
    pinMode(6, OUTPUT);
}
void loop()                  // Main function, sets inputs and pins.
{
   dfultright = bot.ReadRightIRSensor(); //front sensor input
   int dfultleft = bot.ReadLeftIRSensor();  //right sensor input
   int dfult = (dfultright + dfultleft);
   int oldrightnum = 0;
   int currightnum = 0;
   int oldleftnum = 0;
   int curleftnum = 0;


  while(true    /*lightvalue == false*/)
  {

      //bool lightvalue = Checklight();
      int right = 0;
      int front = bot.ReadFrontIRSensor();  //front sensor input
      right = bot.ReadRightIRSensor(); //right sensor input
      int left = bot.ReadLeftIRSensor();  //left sensor input
      
     
      
      Serial.print("\n\ndfult = ");
      Serial.print(dfult);
      Serial.print("\ndfultright = ");
      Serial.print((dfultright));
      Serial.print("\nright = ");
      Serial.print(right);
      Serial.print("\ndfultleft = ");
      Serial.print((dfultleft));
      Serial.print("\nleft = ");
      Serial.print(left);    
      Serial.print("\nfront = ");
      Serial.print(front);

    
//    Serial.print("\noldfront = ");
//    Serial.print(oldfront);   

//    if (lightvalue == true)
//    {
//      targetfound();
//    }

//    oops_goright();
//    leftmotor.write(67);
//    rightmotor.write(77);
//    delay(1500);


      if(front>5000)        //(front-15) < oldfront && oldfront < (front + 15)
      {
         if(currightnum > oldrightnum)
         {
           oops_goleft();
           Serial.print("\noops_goleft "); 
         }
         else if(curleftnum > oldleftnum)
         {
           oops_goright();
           Serial.print("\noops_goright ");
         }
         else
         {
            oldrightnum = currightnum;
            oldleftnum = curleftnum;
         }
      }

      else if ((dfultright-1070)>(right))	    //If the distance from the right wall is over 10 cm
      {

        turnright();			// there is no wall to the right, jump to turnright
        Serial.print("\nturn right ");
        currightnum++;
        
      }

      else if (front<3000)
      {
        moveforward(right);          //there is a wall to the right and no wall forward, move forward
        Serial.print("\nmove forward ");
      }
      else
      {
        turnleft();             //there is a wall to the right, a wall forward, and no wall to the left, turn left
        Serial.print("\nturn left ");
        curleftnum++;
      }
//      else
//      {
//          turnaround();           //there is a wall to the right, a wall forward, a wall to the left, turn around
//          Serial.print("\nturn around ");
//      }

  }
}


bool Checklight()   					//Light sensor function
{
    int x = bot.ReadRightLightSensor();
    int y = bot.ReadFrontLightSensor();
    int z = bot.ReadLeftLightSensor();

    if (x==1 || y==1 || z==1)
    {
        return true;
    }

}



/* 	When the left motor angle is  76 the right motor angle is 81, they are still.
	When the motors angles are over 76 and 81, they rotate forward.
	When the motors angles are  less than 76 and 81, they rotate in reverse.
	The motors rotate until the delay ends, then they are set to 76 and 81. */

void moveforward(int rightvalue)
{
    int fastmotor;
    int slowmotor;
    int timedelay;
     
     
    			//Adjust right forward

    if ((dfultright-800) > rightvalue)
    {
        fastmotor = 90;
        slowmotor = 77;
        timedelay = 280;
        adjustright(fastmotor, slowmotor, timedelay);       //0 to 1.5 cm
        Serial.print(" right 9 ");
        	 	 
    }
        else if ((dfultright-800) <= rightvalue && rightvalue < (dfultright - 780)) 
    {
        fastmotor = 90;
        slowmotor = 77;
        timedelay = 190;
        adjustright(fastmotor, slowmotor, timedelay);       //1.5 to 3 cm
        Serial.print(" right 6 ");
        	 	 
    }
    
    else if ((dfultright-780) <= rightvalue && rightvalue < (dfultright - 680)) 
    {
        fastmotor = 90;
        slowmotor = 77;
        timedelay = 150;
        adjustright(fastmotor, slowmotor, timedelay);       //1.5 to 3 cm
        Serial.print(" right 3 ");
        	 	 
    }

    else if ((dfultright-680) <= rightvalue && rightvalue < (dfultright-400))
    {
        fastmotor = 90;
        slowmotor = 77;
        timedelay = 110;
        adjustright(fastmotor, slowmotor, timedelay);         //4.5 to 6 cm
         Serial.print(" right 1.5 ");      	 	 
    }

            //Move straight forward
   else if ((dfultright+800) >= rightvalue && rightvalue > (dfultright-400))
    {
        fastmotor = 40;
        slowmotor = 90;
        timedelay = 0;
        adjustleft(fastmotor, slowmotor, timedelay);        //6 to 6 both sides
        Serial.print(" straight ");
        	 	 
    }
    	    //Adjust left forward

    else if ((dfultright+800) <= rightvalue && rightvalue < (dfultright +2200)) 
    {
        fastmotor = 40;
        slowmotor = 67;
        timedelay = 45;
        adjustright(fastmotor, slowmotor, timedelay);       //1.5 to 3 cm
        Serial.print(" left 1.5 ");
        	 	 
    }

    else if ((dfultright+2200) <= rightvalue)
    {
        fastmotor = 40;
        slowmotor = 67;
        timedelay = 75;
        adjustright(fastmotor, slowmotor, timedelay);         //4.5 to 6 cm
        Serial.print(" left 3 ");
        	 	 
    }


    
}

void adjustright(int leftspeed, int rightspeed, int timed)
{
    // Adjust the motor to the right depending on the distance away from the wall.
    leftmotor.write(leftspeed);
    rightmotor.write(rightspeed);
    delay(timed);
    leftmotor.write(79);
    rightmotor.write(65);
    delay(700);
    

}

void adjustleft(int rightspeed, int leftspeed, int timed)
{
    rightmotor.write(rightspeed);			// Adjust the motor to the left depending on the distance away from the wall.
    leftmotor.write(leftspeed);
    delay(timed);
    leftmotor.write(79);
    rightmotor.write(65);
    delay(700);
    


}
    




void turnright()
{
    
    leftmotor.write(79);
    rightmotor.write(65);
    delay(1000); 
        
    rightmotor.write(100);		// Left motor rotates forward while right motor rotates in reverse for seconds to turn right.
    leftmotor.write(90);
    delay(370);
    
    rightmotor.write(65);
    leftmotor.write(79);
    delay(1100);

}


void turnleft()				// Left motor rotates in reverse while right motor rotates forwards for seconds to turn left.
{
    rightmotor.write(40);
    leftmotor.write(30);
    delay(352);
    rightmotor.write(65);
    leftmotor.write(79);
    delay(1000);

}

void turnaround()
{
    rightmotor.write(40);		// Left motor rotates in reverse while right motor rotates forwards for 2x seconds to turn around
    leftmotor.write(30);
    delay(900);


}

void oops_goright()
{
    rightmotor.write(100);
    leftmotor.write(67);
    delay(850);
    rightmotor.write(77);
    leftmotor.write(67);
     
}

void oops_goleft()
{
    rightmotor.write(77);
    leftmotor.write(30);
    delay(600);
    rightmotor.write(77);
    leftmotor.write(67);
    
}
//void targetfound()             // If light=1, then target is found, motors stop, led flashes
//{
//    rightmotor.write(81);
//    leftmotor.write(76);
//    while(1)
//    {
//        digitalWrite(6, HIGH);
//        delay(100);
//        digitalWrite(6, LOW);
//        delay(100);
//    }
//}




//Show details


